/*
 * Magnetometer.h
 *
 *  Created on: 23.11.2012
 *      Author: Michael
 */

#ifndef MAGNETOMETER_H_
#define MAGNETOMETER_H_

#include "../Interfaces/I2C.h"

#define ACC_COMPENSATION

#define MAG_ADDR					0x3C
#define ACC_ADDR					0x30

#define CRA_REG_M 					0x00
#define CRB_REG_M 					0x01
#define MR_REG_M 					0x02
#define OUT_X_H_M 					0x03
#define OUT_X_L_M 					0x04
#define OUT_Z_H_M 					0x05
#define OUT_Z_L_M 					0x06
#define OUT_Y_H_M 					0x07
#define OUT_Y_L_M 					0x08
#define SR_REG_M 					0x09
#define IRA_REG_M 					0x0A
#define IRB_REG_M 					0x0B
#define IRC_REG_M 					0x0C

#define CTRL_REG1_A 				0x20
#define CTRL_REG2_A 				0x21
#define CTRL_REG3_A 				0x22
#define CTRL_REG4_A 				0x23
#define CTRL_REG5_A 				0x24
#define HP_FILTER_RESET_A 			0x25
#define REFERENCE_A 				0x26
#define STATUS_REG_A 				0x27
#define OUT_X_L_A 					0x28
#define OUT_X_H_A 					0x29
#define OUT_Y_L_A 					0x2A
#define OUT_Y_H_A 					0x2B
#define OUT_Z_L_A 					0x2C
#define OUT_Z_H_A 					0x2D
#define INT1_CFG_A					0x30
#define INT1_SRC_A 					0x31
#define INT1_THS_A 					0x32
#define INT1_DURATION_A 			0x33
#define INT2_CFG_A 					0x34
#define INT2_SRC_A 					0x35
#define INT2_THS_A 					0x36
#define INT2_DURATION_A				0x37

#define SCALE 						50.f

#define MAG_MAX_X        244
#define MAG_MAX_Y        627
#define MAG_MAX_Z        294
#define MAG_MIN_X        -600
#define MAG_MIN_Y        -330
#define MAG_MIN_Z        -657

struct vector{
	float x;
	float y;
	float z;
};

class Magnetometer {
public:
	Magnetometer(I2C *i2c);
	virtual ~Magnetometer();
	vector getRawVector();
	float getOrientation();
	void readMag();
	void readAcc();
	void setFirstrun(bool firstrun);
private:
	I2C *i2c;
	vector magVector;
	vector refVector;
	vector oldVec;
	float orientation;
	float refLength;

	bool firstrun;
	void vector_cross(const vector *a,const vector *b, vector *out);
	float vector_dot(const vector *a,const vector *b);
	void vector_normalize(vector *a);

	void writeMagReg(uint8_t regAddr, uint8_t val);
	uint8_t readMagReg(uint8_t regAddr);

#ifdef ACC_COMPENSATION
	vector accVector;
	void writeAccReg(uint8_t regAddr, uint8_t val);
	uint8_t readAccReg(uint8_t regAddr);
#endif
};

#endif /* MAGNETOMETER_H_ */
